#include "tanOR.h"
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <fstream>
#include <iostream>

#include <mmintrin.h>
#include <xmmintrin.h>

#define WU_MIN(x,y) x<=y? x:y;
#define WU_MAX(x,y) x>=y? x:y;

void cpVectorDouble2Float(float * p_dest, double *p_src, int num_elem){
	for( int i = 0;i<num_elem; ++i)
		p_dest[i]=(float)p_src[i];
}


void *getPtrToField(const mxArray *p_struct, int i, const char* field_name){
	const mxArray *p_field=mxGetField(p_struct,i,field_name);
	return (void*)mxGetPr(p_field);
}

bool tanOR::initialTanORNode(const mxArray *p_res_tree,float R[], float T[], float swt,const char* tan_path, int i_idx,int i_layer, int i_node, FILE *p_weight){
	this->id = i_idx*1e6+(i_layer+1) * 1e3 + i_node+1;
	// initial rotation matrix for afffine transformation in deformation class
	this->max_rotation = 1;
	R_array = (float**)malloc(sizeof(float*)*(2*max_rotation+1));

	for( int d_O = -max_rotation; d_O<=max_rotation; ++d_O){
		float* R_mat=(float*)malloc(sizeof(float)*4);// 2*2 matrix, lumped by columnwise order
		float theta = d_O*PI/n_orient;
		R_mat[0]=cos(theta);  R_mat[2]=-sin(theta);
		R_mat[1] = sin(theta); R_mat[3]= cos(theta);
		R_array[d_O+max_rotation]= R_mat;
	}


	/*we can update the */
	printf("warining, need to update camera parameters, nLength,nOrient,h\n");
	this->camera.initialize(swt,R,T,n_orient,n_length,h);
	/*fill in the plane file*/
	mxArray *p_plane = mxGetField(p_res_tree,i_node,"plane");
	// get plane parameters, and convert to internal representation


	double *p_tmp_array=(double*)getPtrToField(p_plane,0,"nt");
	cpVectorDouble2Float(&current_plane.nt[0],p_tmp_array,4);
	p_tmp_array = (double*)getPtrToField(p_plane, 0, "origin");
	cpVectorDouble2Float(&current_plane.origin[0],p_tmp_array,3);
	p_tmp_array = (double*)getPtrToField(p_plane, 0, "u");
	cpVectorDouble2Float(&current_plane.bu[0],p_tmp_array,3);
	p_tmp_array = (double*)getPtrToField(p_plane,0,"v");
	cpVectorDouble2Float(&current_plane.bv[0],p_tmp_array,3);
	current_plane.width = *(double*)getPtrToField(p_plane,0,"w");
	current_plane.height = *(double*)getPtrToField(p_plane,0,"h");
	current_plane.stp =*(double*)getPtrToField(p_plane,0,"stp");

	current_plane.height*=current_plane.stp; // plane size in plane coordinate system
	current_plane.width *= current_plane.stp;

	/*upate plane in camera*/
	this->camera.updatePlane(current_plane.bu,current_plane.bv,current_plane.origin,current_plane.nt,current_plane.stp,current_plane.width,current_plane.height);

	/*fill in the shape file*/

	char shape_file_name[1000];
	sprintf(shape_file_name,"%s/learned_shape.txt",tan_path);
	FILE *p_file = fopen(shape_file_name,"r");
	int width,height;

	fscanf(p_file,"%d, %d\n",&width,&height);
	assert(abs(width-current_plane.width)<1);
	assert(abs(height-current_plane.height)<1);

	lineVertex tmp_line;
	while( !feof(p_file)){
		int type,r1,c1,r2,c2;
		fscanf(p_file,"%d, %d, %d, %d, %d\n",&type,&c1,&r1,&c2,&r2);
		switch (type){
			case 1:
				tmp_line.x_start = r1;
				tmp_line.y_start = c1;
				tmp_line.x_end = r2;
				tmp_line.y_end = c2;
				this->tan_shape.line_list.push_back(tmp_line);
				break;
			default:
				fclose(p_file);
				printf("warning, unsupported shape type %d\n",type);
				return false;
		}
	}
	fclose(p_file);
	this->pushDeformations();

	/*fill in the color and flat */
#ifdef USE_COLOR

#ifdef USE_L_COLOR
	char color_file_name[1000];
	sprintf(color_file_name,"%s/color_model.color",tan_path);
	this->readColorModel(const_cast<const char*>(&color_file_name[0]));
#else
	this->initialColorGrid();
#endif

#endif



#ifdef USE_FLAT
	this->initialFlatGrid();
#endif

	/*add feature weight, assume p_weight is a binary file pointer.*/
	p_sk_weight = new float[tan_shape.line_list.size()];
	if(p_weight!=NULL){
		// load feature weights
		if(feof(p_weight)){
			printf("end of coefficient file\n");
		}
		// sketch
		fread(p_sk_weight,sizeof(float),tan_shape.line_list.size(),p_weight);
#ifdef COMPLEXITY_NORMALIZE
		// complexity
		fread(&complexity_weight,sizeof(float),1,p_weight);
#else
		complexity_weight = 0;
#endif

#ifdef USE_COLOR
		fread(&clr_weight,sizeof(float),30,p_weight);
#endif

#ifdef USE_FLAT
		fread(&flat_weight,sizeof(float),10,p_weight);
#endif

	}else{
		// set feature weights to default value
		// sketch
		for(int i_line = 0; i_line<tan_shape.line_list.size(); ++i_line)
			p_sk_weight[i_line]= 1.0f;
		// complexity
		complexity_weight = 0;
		// color
		for(int i= 0; i<30; ++i){
			clr_weight[i]=0.0f;
		}	
		// flat
		for(int i =0; i<10; ++i){
			flat_weight[i]=0.0f;
		}
	}	
	printf("tan weight:");
	for(int i =0; i<tan_shape.line_list.size(); ++i){
		printf("%f,",p_sk_weight[i]);
	}
	printf("\n");
	printf("complexity weight: %f\n",complexity_weight);
#ifdef USE_COLOR
	printf("\n color weight:");
	for(int i =0; i<30; ++i){
		printf("%f,", clr_weight[i]);	
	}
	printf("\n");
#endif


	#ifdef USE_FLAT
	printf("\n flat weight:");
	for(int i =0; i<10; ++i){
		printf("%f,", flat_weight[i]);	
	}
	printf("\n");
#endif

	return true;
}


void tanOR::initialFlatGrid(){
	printf("calling initial flat grid\n");

	int width = this->current_plane.width;
	int height = this->current_plane.height;


	/*center position of current plane*/
	int cx = height/2;
	int cy = width/2;

	/*bounding box for the color patch centers*/
	float lx = height/5.0f;
	float ly = width/5.0f;
	float ux = lx*4;
	float uy = ly*4;

	int separation = 2*h + 1;

	flat_gridX.clear();
	flat_gridY.clear();

	for(float x = lx; x<ux; x+=separation){
		for(float y = ly; y<uy; y+=separation){
			this->flat_gridX.push_back(x);
			this->flat_gridY.push_back(y);
		}
	}

	this->pushFlatDeformations();
}



void tanOR::initialColorGrid(){
	printf("calling initial color grid\n");

	int width = this->current_plane.width;
	int height = this->current_plane.height;


	/*center position of current plane*/
	int cx = height/2;
	int cy = width/2;

	/*bounding box for the color patch centers*/
	float lx = height/5.0f;
	float ly = width/5.0f;
	float ux = lx*4;
	float uy = ly*4;

	int separation = 2*h + 1;

	color_gridX.clear();
	color_gridY.clear();

	for(float x = lx; x<ux; x+=separation){
		for(float y = ly; y<uy; y+=separation){
			this->color_gridX.push_back(x);
			this->color_gridY.push_back(y);
		}
	}

	this->pushColorDeformations();
}


void tanOR::pushColorDeformations(){

	int width =this->current_plane.width;
	int height = this->current_plane.height;
	int cx = height/2;
	int cy = width/2;

	/* then push deformed versions;*/
	//****** remember to change corresponding part in sketch and flat******
	int ddx = (int)(height * 0.10f);
	int ddy = (int)(width * 0.10f);
	int step_x = (int)WU_MAX(ddx,1);
	int step_y = (int)WU_MAX(ddy,1);
	int max_rotation = 1;


	float x,y;// transformed x,y.
	std::vector<float> deform_x,deform_y;
	deform_x.resize(color_gridX.size()); 
	deform_y.resize(color_gridY.size());
	//for( float scale = 0.8f; scale <1.21f; scale = scale + 0.2f){
	float scale = 1.0f; {
		for( int dx = -ddx; dx <=ddx; dx+=step_x){
			for( int dy = -ddy; dy<= ddy; dy+=step_y){
				for( int d_O = -max_rotation; d_O<=max_rotation; ++d_O){
					float theta = d_O*PI/n_orient;
					float R_mat[4];
					R_mat[0]=cos(theta);  R_mat[2]=-sin(theta);
					R_mat[1] = sin(theta); R_mat[3]= cos(theta);

					for(int i_pt=0; i_pt<color_gridX.size(); ++i_pt){
						x = color_gridX[i_pt]+dx-cx;
						y = color_gridY[i_pt]+dy-cy;
						deform_x[i_pt]=(R_mat[0]*x*scale + R_mat[2]*y*scale +cx); // new position, not new deformation
						deform_y[i_pt]=(R_mat[1]*x*scale + R_mat[3]*y*scale +cy); // new position, not new deformation
					}

					this->color_deform_grid_x.push_back(deform_x);
					this->color_deform_grid_y.push_back(deform_y);
				}
			}
		}
	}
}



void tanOR::pushFlatDeformations(){

	int width =this->current_plane.width;
	int height = this->current_plane.height;
	int cx = height/2;
	int cy = width/2;

	/* then push deformed versions;*/
	//****** remember to change corresponding part in sketch and color******
	int ddx = (int)(height * 0.10f);
	int ddy = (int)(width * 0.10f);
	int step_x = (int)WU_MAX(ddx,1);
	int step_y = (int)WU_MAX(ddy,1);
	int max_rotation = 1;


	float x,y;// transformed x,y.
	std::vector<float> deform_x,deform_y;
	deform_x.resize(flat_gridX.size()); 
	deform_y.resize(flat_gridY.size());
	//for( float scale = 0.8f; scale <1.21f; scale = scale + 0.2f){
	float scale = 1.0f; {
		for( int dx = -ddx; dx <=ddx; dx+=step_x){
			for( int dy = -ddy; dy<= ddy; dy+=step_y){
				for( int d_O = -max_rotation; d_O<=max_rotation; ++d_O){
					float theta = d_O*PI/n_orient;
					float R_mat[4];
					R_mat[0]=cos(theta);  R_mat[2]=-sin(theta);
					R_mat[1] = sin(theta); R_mat[3]= cos(theta);

					for(int i_pt=0; i_pt<flat_gridX.size(); ++i_pt){
						x = flat_gridX[i_pt]+dx-cx;
						y = flat_gridY[i_pt]+dy-cy;
						deform_x[i_pt]=(R_mat[0]*x*scale + R_mat[2]*y*scale +cx); // new position, not new deformation
						deform_y[i_pt]=(R_mat[1]*x*scale + R_mat[3]*y*scale +cy); // new position, not new deformation
					}

					this->flat_deform_grid_x.push_back(deform_x);
					this->flat_deform_grid_y.push_back(deform_y);
				}
			}
		}
	}

}


void tanOR::pushDeformations(){
	int width = this->current_plane.width;
	int height = this->current_plane.height;
	int cx =height/2;
	int cy =width/2;
	// push thedeformations

	// following deformation are within a +-10% *height
	// remember to change corresponding part in flat and color
	int ddx = (int)(height * 0.10f);
	int ddy = (int)(width * 0.10f);
	int step_x = (int)MAX(ddx,1);
	int step_y = (int)MAX(ddy,1);

	activeShape &shape= tan_shape;
	shape.deformation_list.resize(0);
	lineVertex tmp_line;
	float x,y,tx,ty;// transformed x,y.
	deformation tmp_deformation;
	//for( float scale = 0.8; scale <1.21; scale = scale + 0.2){
	float scale = 1.0f; {
		for( int dx = -ddx; dx <=ddx; dx+=step_x){
			for( int dy = -ddy; dy<= ddy; dy+=step_y){
				for( int d_O = -max_rotation; d_O<=max_rotation; ++d_O){
					tmp_deformation.line_list.clear();
					for( int i_line = 0; i_line<shape.line_list.size(); ++i_line){
						//printf("dx %d, dy %d, scale %f, dO: %d\n",dx,dy,scale,d_O);
						float *p_R = this->R_array[d_O+max_rotation];
						x = shape.line_list[i_line].x_start+dx-cx;
						y = shape.line_list[i_line].y_start+dy-cy;
						tx=(p_R[0]*x*scale + p_R[2]*y*scale +cx); // new position, not new deformation
						ty=(p_R[1]*x*scale + p_R[3]*y*scale +cy); // new position, not new deformation
						tmp_line.x_start = tx;
						tmp_line.y_start = ty;


						x = shape.line_list[i_line].x_end+dx-cx;
						y = shape.line_list[i_line].y_end+dy-cy;
						tx=(p_R[0]*x*scale + p_R[2]*y*scale +cx); // new position, not new deformation
						ty=(p_R[1]*x*scale + p_R[3]*y*scale +cy); // new position, not new deformation
						tmp_line.x_end = tx;
						tmp_line.y_end = ty;

						tmp_deformation.line_list.push_back(tmp_line);
					}// i_line
					shape.deformation_list.push_back(tmp_deformation);
				}// d_Orient
			}// dx
		}// dy
	}// scale
	//int n_deform = shape_deque[i_shape].deformation_list.size();
	//printf(" width : %d, height %d, deformations %d\n",width,height,n_deform);
}



bool tanOR::release(){

	delete[] p_sk_weight;
	camera.release();
	for( int d_O = -max_rotation; d_O<=max_rotation; ++d_O){
		free(R_array[d_O+max_rotation]);
	}
	free(R_array);
	return true;
}

bool tanOR::inference(line &mat_line_1,line &mat_line_2){

	/*when line_list.size()>4, it is no longer a four side shape, thus P is not computable from it.*/
	if( this->tan_shape.line_list.size()>4){
		return false;
	}
	lineVertex l1,l2;
	l1 = p_img_data->matlabLine2lineVertex(mat_line_1);
	l2 = p_img_data->matlabLine2lineVertex(mat_line_2);
	/*returned line l1,l2 is in image coordinate system*/


	return inference(l1,l2);
}

void tanOR::proposeParticle(const lineVertex &line1, const lineVertex &line2, tanParticle_s &ptc1, tanParticle_s &ptc2){
	matchLine(line1,line2,ptc1.pt_pairs,ptc2.pt_pairs);

	//camera.estimatePFast(ptc1.pt_pairs,ptc1.P,ptc1.cam_dir,ptc1.fit_error);
	camera.estimatePerspectiveTransform(ptc1.pt_pairs,ptc1.P,ptc1.cam_dir,ptc1.fit_error);
	this->test(ptc1.P,ptc1.cam_dir,ptc1.score,ptc1.arg_max_deformation);

	camera.estimatePerspectiveTransform(ptc2.pt_pairs,ptc2.P,ptc2.cam_dir,ptc2.fit_error);
	//camera.estimatePFast(ptc1.pt_pairs,ptc1.P,ptc1.cam_dir,ptc1.fit_error);
	this->test(ptc2.P,ptc2.cam_dir,ptc2.score,ptc2.arg_max_deformation);
}

bool tanOR::inference(lineVertex &line1, lineVertex &line2){

	/*trying to match 3D points to 2D points*/
	/*
	float P[3][4];
	float dir[3];
	float error;
	float score;
	*/

	tanParticle_s ptc1,ptc2;

	proposeParticle(line1,line2,ptc1,ptc2);

	updateParticleList(ptc1);
	updateParticleList(ptc2);
	return true;
}


bool tanOR::updateParticleList(tanParticle_s & particle){
	if( particle.score<-1e6) return false;
	if( this->tan_particle_list.empty()){
		tan_particle_list.push_back(particle);
		return true;
	}
	std::list<tanParticle_s>::iterator itr = tan_particle_list.begin();
	for(;itr!=tan_particle_list.end(); ++itr){
		if(particle.score>=itr->score){
			tan_particle_list.insert(itr,particle);
			if(tan_particle_list.size()>PR_QUEUE_SIZE){
				tan_particle_list.pop_back();
			}
			return true;
		}
	}
	if(tan_particle_list.size()<PR_QUEUE_SIZE){
		tan_particle_list.push_back(particle);
		return true;
	}
	return false;

}



void tanOR::matchLine(const lineVertex &img_l1, const lineVertex &img_l2, std::deque<pt_pair_s> &pair_1, std::deque<pt_pair_s> &pair_2){
	pair_1.resize(4);
	pair_2.resize(4);

	line3D L1,L2;

	lineVertex &l1 = tan_shape.line_list[0];
	lineVertex &l2 = tan_shape.line_list[1];

	float cx,cy; /*center of the shape*/
	cx = (l1.x_start+l1.x_end+l2.x_start+l2.x_end)/4.0f;
	cy = (l1.y_start+l1.y_end+l2.y_start+l2.y_end)/4.0f;
	/*caution about the coordinate system*/
	/*here we assume l1 and l2 is in  plane coordinate system*/
	camera.planePoint2World(l1.x_start,l1.y_start,L1.x1,L1.y1,L1.z1);
	camera.planePoint2World(l1.x_end,l1.y_end,L1.x2,L1.y2,L1.z2);

	camera.planePoint2World(l2.x_start,l2.y_start,L2.x1,L2.y1,L2.z1);
	camera.planePoint2World(l2.x_end,l2.y_end,L2.x2,L2.y2,L2.z2);

	/*check relative orientation of template lines*/
	double theta1 = pointAngle((l1.y_start-cy),-(l1.x_start-cx));
	double theta2 = pointAngle((l1.y_end-cy),-(l1.x_end-cx));
	assert(theta1!=theta2);

	if(theta1>theta2){
		pair_1[0][0] = L1.x2;pair_1[0][1] = L1.y2;pair_1[0][2] = L1.z2;
		pair_1[1][0] = L1.x1;pair_1[1][1] = L1.y1;pair_1[1][2] = L1.z1;

		pair_2[0][0] = L1.x2;pair_2[0][1] = L1.y2;pair_2[0][2] = L1.z2;
		pair_2[1][0] = L1.x1;pair_2[1][1] = L1.y1;pair_2[1][2] = L1.z1;
	}
	else{
		pair_1[0][0] = L1.x1;pair_1[0][1] = L1.y1;pair_1[0][2] = L1.z1;
		pair_1[1][0] = L1.x2;pair_1[1][1] = L1.y2;pair_1[1][2] = L1.z2;

		pair_2[0][0] = L1.x1;pair_2[0][1] = L1.y1;pair_2[0][2] = L1.z1;
		pair_2[1][0] = L1.x2;pair_2[1][1] = L1.y2;pair_2[1][2] = L1.z2;
	}



	theta1 = pointAngle((l2.y_start-cy),-(l2.x_start-cx));
	theta2 = pointAngle((l2.y_end-cy),-(l2.x_end-cx));
	assert(theta1!=theta2);

	if(theta1>theta2){
		pair_1[2][0] = L2.x2;pair_1[2][1] = L2.y2;pair_1[2][2] = L2.z2;
		pair_1[3][0] = L2.x1;pair_1[3][1] = L2.y1;pair_1[3][2] = L2.z1;

		pair_2[2][0] = L2.x2;pair_2[2][1] = L2.y2;pair_2[2][2] = L2.z2;
		pair_2[3][0] = L2.x1;pair_2[3][1] = L2.y1;pair_2[3][2] = L2.z1;
	}
	else{
		pair_1[2][0] = L2.x1;pair_1[2][1] = L2.y1;pair_1[2][2] = L2.z1;
		pair_1[3][0] = L2.x2;pair_1[3][1] = L2.y2;pair_1[3][2] = L2.z2;

		pair_2[2][0] = L2.x1;pair_2[2][1] = L2.y1;pair_2[2][2] = L2.z1;
		pair_2[3][0] = L2.x2;pair_2[3][1] = L2.y2;pair_2[3][2] = L2.z2;
	}

	/*check and fill in image lines*/
	/*we assume image line is in image coorindate*/
	/*which means origin is at image center*/
	cx = (img_l1.x_start+img_l1.x_end+img_l2.x_start+img_l2.x_end)/4.0f;
	cy = (img_l1.y_start+img_l1.y_end+img_l2.y_start+img_l2.y_end)/4.0f;

	theta1 = pointAngle((img_l1.x_start-cx),-(img_l1.y_start-cy));
	theta2 = pointAngle((img_l1.x_end-cx), -(img_l1.y_end-cy));
	//	assert(theta1!=theta2);

	if(theta1>theta2){
		pair_1[0][3]=img_l1.x_end;   pair_1[0][4]=img_l1.y_start;
		pair_1[1][3]=img_l1.x_start; pair_1[1][4]=img_l1.y_end;

		pair_2[2][3]=img_l1.x_end;   pair_2[2][4]=img_l1.y_start;
		pair_2[3][3]=img_l1.x_start; pair_2[3][4]=img_l1.y_end;
	}else{
		pair_1[0][3]=img_l1.x_start;   pair_1[0][4]=img_l1.y_start;
		pair_1[1][3]=img_l1.x_end;	   pair_1[1][4]=img_l1.y_end;

		pair_2[2][3]=img_l1.x_start;   pair_2[2][4]=img_l1.y_start;
		pair_2[3][3]=img_l1.x_end;     pair_2[3][4]=img_l1.y_end;	
	}

	theta1 = pointAngle((img_l2.x_start-cx),-(img_l2.y_start-cy));
	theta2 = pointAngle((img_l2.x_end-cx), -(img_l2.y_end-cy));
	//	assert(theta1!=theta2);
	if(theta1>theta2){
		pair_1[2][3] = img_l2.x_end;	pair_1[2][4] = img_l2.y_end;
		pair_1[3][3] = img_l2.x_start;	pair_1[3][4] = img_l2.y_start;

		pair_2[0][3] = img_l2.x_end;	pair_2[0][4] = img_l2.y_end;
		pair_2[1][3] = img_l2.x_start;  pair_2[1][4] = img_l2.y_start;
	}else{
		pair_1[2][3] = img_l2.x_start;	pair_1[2][4] = img_l2.y_start;
		pair_1[3][3] = img_l2.x_end;	pair_1[3][4] = img_l2.y_end;

		pair_2[0][3] = img_l2.x_start;	pair_2[0][4] = img_l2.y_start;
		pair_2[1][3] = img_l2.x_end;	pair_2[1][4] = img_l2.y_end;
	}


}

/*compute the angle of the point in polar coordinate system*/
/*assume input point is like Euler coordinate system*/
double tanOR::pointAngle(float x, float y){
	double theta = atan2(y,x); 
	if(theta<0)
		theta +=2*PI;
	return theta;
}


bool tanOR::test(float P[][4], float cam_dir[], float &score, int &arg_max){

	int sx = p_img_data->sx;
	int sy = p_img_data->sy;
	float **M2_maps = p_img_data->M2map;

	float P_vec[12];
	/*put it into columnwise order*/
	for( int r = 0;r<3; ++r){
		for( int c = 0; c<4; ++c){
			P_vec[c*3+r]=P[r][c];
		}
	}

	score = NEGMAX;
	arg_max = -1;

	/*
	* code below are from class shapeAndResponse
	*/

	lineVertex image_line;
	line matlab_line;
	int i_page,i_pix;
	float current_score=0;
	// update image projection matrix;
	this->camera.updateCamera(P_vec,cam_dir,1.f,1.f);
	if (!camera.isVisible()) {
		//printf("proposed matrix not visible, will return");
		score = 0;
		arg_max = -1;
		return false;
	}

	activeShape &current_shape = tan_shape;
	for(int i_deform = 0; i_deform<current_shape.deformation_list.size();++i_deform){
		deformation &current_deformation=current_shape.deformation_list[i_deform];
		// for each deformation
#ifdef SHOW_SHAPE_PROJECTION
		char name[512];
		sprintf(name,"../cache/image_%d_shape_%d_deform%d.pshape",i_img,i_shape,i_deform);
		svgDrawer drawer(n_orient);
		const char *p_name = const_cast<const char*> (name);
		drawer.newImage(p_name,p_Sy[i_img],p_Sx[i_img]);
#endif
		current_score = 0;
		int num_segments =0;
		for(int i_line =0; i_line<current_shape.line_list.size();++i_line){

			lineVertex &my_line = current_deformation.line_list[i_line];
			// compute score
			/*
			*conversion from plane line to image line, with both x axiss goes top-down, y axis goes left-right, 
			*so , image_line is not strictly following the image coordiate system we defined.
			*/
			camera.planeLineVertex2MatlabImage(my_line, image_line);
			// this is only convert the representation from struct line, to struct line2Vertex
			/*after the conversion below, matlab line is origin center, x top-down and y left-right*/
			matlab_line=p_img_data->lineVertex2Line(image_line); 
			/*then, we offset the origin to  top-left corner*/
			matlab_line.x = ROUND(matlab_line.x + 0.5f*sx+1);
			matlab_line.y = ROUND(matlab_line.y + 0.5f*sy+1);
			if (matlab_line.length>=n_length){
				// printf("line length greater than prepared %d\n",matlab_line.length);
				matlab_line.length = n_length-1;
			}

			num_segments +=matlab_line.length*2+1;
#ifdef SHOW_SHAPE_PROJECTION
			lineVertex tmp_line;
			tmp_line.x_end=image_line.y_end;
			tmp_line.x_start=image_line.y_start;
			tmp_line.y_end=image_line.x_end;
			tmp_line.y_start=image_line.x_start;
			drawer.pushLine(tmp_line);
#endif
			// check if out of boundary
			if(matlab_line.x<=0 || matlab_line.x>sx || matlab_line.y<=0 || matlab_line.y>sy ){
				current_score = NEGMAX;
				continue;
			}
			i_page=sub2IndS2(0,matlab_line.orient,0,matlab_line.length);
			i_pix = matlab_line.x-1+(matlab_line.y-1)*sx;

			assert(i_pix<sx*sy);
			assert(i_pix>=0);
			assert(i_page>=0);
			assert(i_page<n_length*n_orient*1);
#ifdef LENGTH_NORMALIZE
			float length = matlab_line.length*2+1.0f;
			current_score +=p_sk_weight[i_line]*M2_maps[i_page][i_pix]/length;
#else
			current_score +=p_sk_weight[i_line]*M2_maps[i_page][i_pix];
#endif
		}// i line



#ifdef COMPLEXITY_NORMALIZE
		current_score += complexity_weight*num_segments;
#endif

#ifdef USE_COLOR
		float color_score = this->testColor(i_deform);
		current_score += color_score;
#endif

		if (fabs(current_score)>1e3){
			current_score = NEGMAX;
		}
		if( score<=current_score){
			score = current_score;
			arg_max = i_deform;
		}

#ifdef SHOW_SHAPE_PROJECTION
		drawer.saveImage();
#endif
	}// deform
	//printf("id: %d,score %f\n",this->id,score);
	return true;
}


int tanOR::sub2IndS2(int i_img, int i_orient, int i_angle, int i_length){
	// M2 map is in format of [ nOrient nLength n_image]
	int i = 0;
	i += i_img*(n_orient*n_length);
	i += i_orient*n_length;
	i += i_length;
	return i;
}




void tanOR::updateImageDataRef(imgData *p){
	this->p_img_data=p;
	this->camera.updataFilterParam(p->n_orient,p->n_length,p->h);
	this->h = p->h;
	this->n_length = p->n_length;
	this->n_orient = p->n_orient;
	this->irac.release();
	this->irac.initial(p);
}


float tanOR::testColor(int i_deform, float * p_color_feature_vec){
	int n_pt = color_gridX.size();
	if(n_pt==0){	
		if(p_color_feature_vec!=NULL) 
			memset(p_color_feature_vec,0,sizeof(float)*30);
		return 0;
	}

	/*assume the camera is updated, and the current plane is visible*/
	/*variance is computed as: $V[x]=E[x^2]-(E[x])^2$ */

	int x,y; float fx,fy;
	int l,a,b;
	float tmp_hist[30];
	memset(&tmp_hist,0,sizeof(float)*30);
	float mean_l, mean_a, mean_b;
	float var_l,var_a,var_b;
	int cx,cy;
	float score=0;
	int here;
	mean_a=0.0f;mean_b=0.f;mean_l = 0.f;
	var_l = 0.0f;var_a =0.0f; var_b = 0.0f;

	int sx = p_img_data->sx;
	int sy = p_img_data->sy; 

	cx = floor(sx/2.0f);
	cy = floor(sy/2.0f);



	std::vector<float> &deform_grid_x = color_deform_grid_x[i_deform];
	std::vector<float> &deform_grid_y = color_deform_grid_y[i_deform];
	for( int ipt =0; ipt<deform_grid_x.size(); ++ipt){
		camera.planePoint2Image(deform_grid_x[ipt],deform_grid_y[ipt],fy,fx);
		x = fx + cx;
		y = fy + cy;
		if(x<0||x>=sx||y<0||y>=sy){
			continue;
		}
		here = y*sx + x;

		a = p_img_data->color_a_map[here];
		b = p_img_data->color_b_map[here];
		l = p_img_data->color_l_map[here];


		mean_l += l;
		mean_a +=(a-8); 
		mean_b+=(b-16);
		var_l += l*l; 
		var_a +=(a-8)*(a-8); 
		var_b += (b-16)*(b-16);

		++tmp_hist[l];
		++tmp_hist[a]; 
		++tmp_hist[b];
	}
	float coef = 1.0f/(float)(color_gridX.size());
	float norm_const = 10*coef;
	for (int i_bin =0; i_bin<24; ++i_bin) {
		tmp_hist[i_bin]*=norm_const;
	}
	mean_l *=coef;mean_a*=coef;mean_b*=coef;
	var_l = var_l*coef - (mean_l*mean_l);
	var_a = var_a*coef - (mean_a*mean_a);
	var_b = var_b*coef - (mean_b*mean_b);

	tmp_hist[24]=mean_l; tmp_hist[25]=mean_a; tmp_hist[26]=mean_b;
	tmp_hist[27]=var_l;  tmp_hist[28]=var_a;  tmp_hist[29]=var_b;


	for(int i_bin = 0; i_bin<30; ++i_bin){
		score += clr_weight[i_bin]*tmp_hist[i_bin];
	}
	if(p_color_feature_vec!=NULL){
		memcpy(p_color_feature_vec,&tmp_hist[0],sizeof(float)*30);
	}

	return score;

}



float tanOR::testFlat(int i_deform, float * p_flat_feature_vec){
	int n_pt = flat_gridX.size();
	if(n_pt==0){	
		if(p_flat_feature_vec!=NULL) 
			memset(p_flat_feature_vec,0,sizeof(float)*10);
		return 0;
	}

	/*assume the camera is updated, and the current plane is visible*/
	/*variance is computed as: $V[x]=E[x^2]-(E[x])^2$ */

	int x,y; float fx,fy;
	int flat;
	float tmp_hist[10];
	memset(&tmp_hist,0,sizeof(float)*10);
	float mean, var;
	int cx,cy;
	float score=0;
	int here;
	mean = 0; var = 0;

	int sx = p_img_data->sx;
	int sy = p_img_data->sy; 

	cx = floor(sx/2.0f);
	cy = floor(sy/2.0f);

	std::vector<float> &deform_grid_x = flat_deform_grid_x[i_deform];
	std::vector<float> &deform_grid_y = flat_deform_grid_y[i_deform];
	for( int ipt =0; ipt<deform_grid_x.size(); ++ipt){
		camera.planePoint2Image(deform_grid_x[ipt],deform_grid_y[ipt],fy,fx);
		x = fx + cx;
		y = fy + cy;
		if(x<0||x>=sx||y<0||y>=sy){
			continue;
		}
		here = y*sx + x;

		flat = p_img_data->mo_map[here];

		mean+=flat;
		var+=flat*flat;

		++tmp_hist[flat];
	}
	float coef = 1.0f/(float)(flat_gridX.size());
	float norm_const = 10*coef;
	for (int i_bin =0; i_bin<8; ++i_bin) {
		tmp_hist[i_bin]*=norm_const;
	}
	mean*=coef;
	var = var*coef - (mean*mean);

	tmp_hist[8]=mean;
	tmp_hist[9]=var;


	for(int i_bin = 0; i_bin<10; ++i_bin){
		score += flat_weight[i_bin]*tmp_hist[i_bin];
	}
	if(p_flat_feature_vec!=NULL){
		memcpy(p_flat_feature_vec,&tmp_hist[0],sizeof(float)*10);
	}

	return score;

}

void tanOR::drawFlat(int arg_max,float* p_template){
	int sx = p_img_data->sx;
	int sy = p_img_data->sy;
	int dx,dy,here;
	int cx,cy;
	float fx,fy;
	int x,y;

	cx = floor(sx/2.0f);
	cy = floor(sy/2.0f);


	std::vector<float> &deform_grid_x = flat_deform_grid_x[arg_max];
	std::vector<float> &deform_grid_y = flat_deform_grid_y[arg_max];
	for( int ipt =0; ipt<deform_grid_x.size(); ++ipt){
		camera.planePoint2Image(deform_grid_x[ipt],deform_grid_y[ipt],fy,fx);
		x = fx + cx;
		y = fy + cy;
		if(x<0||x>=sx||y<0||y>=sy){
			continue;
		}
		for(dx = x-2; dx<=x+2; ++dx){
			for(dy = y-2; dy<=y+2; ++dy){
				if(dx<0||dx>sx||dy<0||dy>=sy) continue;
				here = dy*sx + dx;
				p_template[here]=0.5;
			}
		} 
		here = y*sx + x;
	}
}

void tanOR::drawColor(int arg_max,float* p_template){
	int sx = p_img_data->sx;
	int sy = p_img_data->sy;
	int dx,dy,here;
	int cx,cy;
	float fx,fy;
	int x,y;

	cx = floor(sx/2.0f);
	cy = floor(sy/2.0f);


	std::vector<float> &deform_grid_x = color_deform_grid_x[arg_max];
	std::vector<float> &deform_grid_y = color_deform_grid_y[arg_max];
	for( int ipt =0; ipt<deform_grid_x.size(); ++ipt){
		camera.planePoint2Image(deform_grid_x[ipt],deform_grid_y[ipt],fy,fx);
		x = fx + cx;
		y = fy + cy;
		if(x<0||x>=sx||y<0||y>=sy){
			continue;
		}
		for(dx = x-2; dx<=x+2; ++dx){
			for(dy = y-2; dy<=y+2; ++dy){
				if(dx<0||dx>sx||dy<0||dy>=sy) continue;
				here = dy*sx + dx;
				p_template[here]=0.5;
			}
		} 
		here = y*sx + x;
	}
}

void tanOR::readColorModel(const char *path_name){

	color_gridX.clear();
	color_gridY.clear();

	int n_bin = n_color_bin;

	std::ifstream File;
	File.open(path_name,std::ios::in);
	if(!File.is_open()){
		printf("Cannot open file, %s\n",path_name);
		return;
	}

	char *p_trash = (char*)malloc(sizeof(char)*1000);
	std::stringstream line;
	std::string digit;
	int tmp_w,tmp_h;
	float x,y;

	File>>tmp_w;
	File.ignore();
	File>>tmp_h;
	File.getline(p_trash,1000);	


	while(!File.eof()){
		float type;

		File>>type;
		File.ignore();
		if(File.eof()) break;

		File>>x;
		File.ignore();

		File>>y;
		File.ignore();

		/*read and throw away the rest of line */
		File.getline(p_trash,1000);

		color_gridX.push_back(x);
		color_gridY.push_back(y);
		printf("x:%f,y:%f\n",x,y);
	}

	free(p_trash);
	File.close();

	pushColorDeformations();

	printf("num color points %d.\n",(int)color_gridX.size());
}

